#include <iostream>
#include <ros/ros.h>
#include "03_YunleCAN_A.h"
#include "autoware_msgs/VehicleStatus.h"
#include "autoware_msgs/VehicleCmd.h"
#include "tablet_socket_msgs/mode_cmd.h"
#include "can_device_interface/canmsg.h"
//#include "plusgo_bywire.h"
/*
设计者：唐明弘
邮箱：tangmh_2017@163.com
完成时间：2023.04.23

主要思路：
首先声明控制量和状态量作为全局变量，然后在while循环中每隔固定周期向can bridge和autoware发送控制量和状态量
同时每个周期调用spinOnce，检测是否有从can bridge和wutoware来的数据，如果有就翻译这些数据，分别赋给控制量和状态量
*/
ros::Subscriber vehicle_control_info_sub; // 从上层接收控制指令
ros::Publisher vehicle_control_info_pub;  // 向下层发送控制指令

ros::Publisher vehicle_state_info_pub;  // 向上层发送状态数据
ros::Subscriber vehicle_state_info_sub; // 从下层接收状态数据
ros::Subscriber autopilot_mode_sub;//监听图形界面的autopilot按钮状态，控制车辆自动驾驶功能的使能

can_device_interface::canmsg out_vehicle_cmd;
autoware_msgs::VehicleStatus vehicle_status;

int out_vehicle_cmd_counter=0;

can_0x051_CCU_Status_t can_vehicle_status;//车辆状态全局变量
can_0x120_SCU_t can_vehicle_cmd;//车辆控制信号全局变量


void vehicle_control_info_sub_callback(const autoware_msgs::VehicleCmd::ConstPtr &msg);
void vehicle_state_info_sub_callback(const can_device_interface::canmsg::ConstPtr &msg);
void autopilot_mode_sub_callback(const tablet_socket_msgs::mode_cmd::ConstPtr &msg);


int main(int argc,char** argv)
{
    ros::init(argc, argv, "can_ad_interface_node");
    ros::NodeHandle n;
    vehicle_control_info_sub=n.subscribe("vehicle_cmd",10,vehicle_control_info_sub_callback);//从autoware订阅控制topic
    vehicle_control_info_pub=n.advertise<can_device_interface::canmsg>("control_msg",10);//向can_device_interface节点发送根据dbc转换完的控制topic
    vehicle_state_info_sub = n.subscribe("can_raw", 10, vehicle_state_info_sub_callback);    // 从can_device_interface节点订阅车辆状态topic
    vehicle_state_info_pub=n.advertise<autoware_msgs::VehicleStatus>("vehicle_status",10);//向autoware发送根据dbc转换完的车辆状态topic
    autopilot_mode_sub=n.subscribe("mode_cmd",1,autopilot_mode_sub_callback);
    ros::Rate loop_rate(10);
    while(ros::ok)
    {
        //发送周期10ms一次
        vehicle_control_info_pub.publish(out_vehicle_cmd);//向can_bridge发送控制帧
        vehicle_state_info_pub.publish(vehicle_status);//向autoware发送状态数据
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

void autopilot_mode_sub_callback(const tablet_socket_msgs::mode_cmd::ConstPtr &msg)
{   //当自动驾驶使能，要做的事：
    //1. 底盘自动驾驶使能 Drive_Mode_Req=1
    //2. 底盘换到前进挡 ShiftLevel_Req=1
    //当自动驾驶使能取消，要做的事：
    //1. 底盘自动驾驶使能取消 Drive_Mode_Req=0
    //2. 底盘换到空挡 ShiftLevel_Req=2
    can_vehicle_cmd.SCU_Drive_Mode_Req=msg->mode;//CAN控制模式使能
    if (msg->mode==1)
        can_vehicle_cmd.SCU_ShiftLevel_Req=1;//前进挡
    else
        can_vehicle_cmd.SCU_ShiftLevel_Req=2;//空挡

    ROS_INFO("Autopilot Status:%d",msg->mode);
}

void vehicle_control_info_sub_callback(const autoware_msgs::VehicleCmd::ConstPtr &msg)
{
    //***************************************************************************
    //数据头处理
    out_vehicle_cmd.count=out_vehicle_cmd_counter;
    out_vehicle_cmd.flag=can_vehicle_cmd.SCU_Drive_Mode_Req;
    out_vehicle_cmd.header.frame_id='control_can';
    out_vehicle_cmd.header.seq=out_vehicle_cmd_counter;
    out_vehicle_cmd.header.stamp=ros::Time::now();
    out_vehicle_cmd.id=0x120;
    out_vehicle_cmd.len=8;
    out_vehicle_cmd.time=out_vehicle_cmd_counter;
    
    //***************************************************************************
    //数据本体处理（out_vehicle_cmd.data）,将autoware的期望加速度、转角等转化成64位2进制数
    //autoware输出期望加速度和速度，底盘接受期望速度
    can_vehicle_cmd.SCU_Target_Speed=msg->ctrl_cmd.linear_velocity;// 1 km/h=3.6 m/s
    //转角转换，底盘接收的是方向盘转角，它和前轮转角的转换比例是 120/27 转角单位deg，
    //autoware输出的期望转角是前轮转角  转角单位是rad
    can_vehicle_cmd.SCU_Steering_Wheel_Angle=120*(180*msg->ctrl_cmd.steering_angle/3.14)/27;

    ROS_INFO("desired speed = %f,desired steer angle = %f",can_vehicle_cmd.SCU_Target_Speed,can_vehicle_cmd.SCU_Steering_Wheel_Angle);
    

    if (msg->ctrl_cmd.steering_angle<-1*3.14/180)//方向盘转角单位根据状态数据单位应该是rad
        can_vehicle_cmd.GW_Left_Turn_Light_Req=1;//左转向灯使能
    if (msg->ctrl_cmd.steering_angle>10*3.14/180)
        can_vehicle_cmd.GW_Right_Turn_Light_Req=1;//右转向灯使能
    //把物理数据打包成64位二进制CAN数据
    uint64_t data;
    pack_can_0x120_SCU(&can_vehicle_cmd, &data);
    //***************************************************************************
    //把64位二进制CAN数据转换成8个8位的2进制  
    for (int j = 0; j < out_vehicle_cmd.len; j++)
    {
        out_vehicle_cmd.data[j] = (data >> ((j)*8)) & 0xff; // 这一步实现数据从高位向低位的排列，两种格式应该是没区别的
    }

    //重置计数器
    if (out_vehicle_cmd_counter>=10000)
        out_vehicle_cmd_counter=0;
    else
        out_vehicle_cmd_counter++;
    

}

void vehicle_state_info_sub_callback(const can_device_interface::canmsg::ConstPtr &msg)
{
    //把can_bridge得到的车辆状态数据解包成物理数据传给autoware
    //ROS_INFO("msg id %d, msg channel %d",msg->id,msg->channel_id);
    if (msg->channel_id==1 && msg->id==0x51)
    {
        //*****************************************************************************
        //数据解包
        uint64_t data = 0;
        
        for (int j = 0; j < msg->len; j++)
        {
            data =  data << 8;
            data |=  msg->data[7-j];
        }
        unpack_can_0x051_CCU_Status(&can_vehicle_status, data, msg->len);
        //*****************************************************************************
        //数据赋值
        //前轮转角，autoware接口为前轮转角，CAN数据是方向盘角度和方向，
        vehicle_status.angle=can_vehicle_status.CCU_Steering_Wheel_Angle*27/120;
        if (can_vehicle_status.Steering_Wheel_Direction=0)//左边,方向盘左负右正
            vehicle_status.angle=-vehicle_status.angle;
        vehicle_status.drivemode = can_vehicle_status.CCU_Drive_Mode;
        vehicle_status.lamp = can_vehicle_status.Hazard_Light_Sts || can_vehicle_status.Left_Turn_Light_Sts || can_vehicle_status.Right_Turn_Light_Sts;
        //vehicle_status.LAMP_LEFT = can_vehicle_status.Left_Turn_Light_Sts;
        //vehicle_status.LAMP_RIGHT = can_vehicle_status.Right_Turn_Light_Sts;
        //vehicle_status.LAMP_HAZARD = can_vehicle_status.Hazard_Light_Sts;
        vehicle_status.steeringmode = can_vehicle_status.CCU_Drive_Mode;
        // 车速,m/s=km/h/3.6
        //!!!!注意，这里dbc标注的速度单位是m/s，但0x120标注的速度单位是km/h，需要实车测试的时候确认一下
        vehicle_status.speed = can_vehicle_status.CCU_Vehicle_Speed;

        ROS_INFO("vehicle speed is %f, and vehicle steering angle is %f",vehicle_status.speed,vehicle_status.angle);
    }
    
}